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Abstract-In this paper, two methods are presented for solving 
closed loop optimal control problem and finding dynamic load 
carrying capacity (DLCC) for fixed and mobile manipulators. 
These control laws are based on the numerical solution to 
nonlinear Hamilton- Jacobi-Bellman (HJB) equation. First 
approach is the Successive Approximation (SA) for finding 
solution of HJB equation in the closed loop form and second 
approach is based on solving state-dependent Riccati equation 
(SDRE) that is an extension of algebraic Riccati equation for 
nonlinear systems. Afterward dynamic load carrying capacity 
of manipulators is computed using these controllers. The 
DLCC is calculated by considering tracking error and limits 
of torque's joints. Finally, results are presented for two cases, 
a two-link planar manipulator mounted on a differentially 
driven mobile base and a 6DOF articulated manipulator (6R). 
The simulation results are verified with the experimental 
test for the 6R manipulator. The simulation and experimental 
results demonstrate that these methods are convenient for 
finding nonlinear optimal control laws in state feedback form 
and finding the maximum allowable load on a given trajectory. 

Index Terms — manipulator, DLCC, optimal feedback, SDRE, 
SA 



I. Introduction 

The DLCC of manipulators is one of the important 
characteristics of manipulators that restricted by limits of 
motor torques and tracking accuracy. In [1] a technique is 
introduced for computing the DLCC based on linear 
programming (LP) and then the DLCC is determined for 
specified path. In [2, 3] the DLCC problem is converted to an 
optimization problem and iterative linear programming (ILP) 
is used for solving this subject, and then this method is 
employed to find the DLCC of flexible manipulators and mobile 
robots. Korayem and Nikoobin [4] calculated maximum 
allowable load of mobile manipulator for a point-to-point 
motion by applying open loop optimal control approach. 

Closed loop methods are used for determining Load 
carrying capacity, in [5] finding the DLCC of flexible joint 
robots is presented via the sliding mode control. Korayem et 
al [6] computed load carrying capacity of flexible joint 
manipulators using feedback linearization approach.Limitation 
on motor torques is one of factors that restricts the DLCC on 
a given trajectory, whatever the torque's motors are 
decreased, the DLCC will be increased, so using closed loop 
optimal control can increase the quantity of load capacity. 
Ravan and poulsen [7] presented analysis and design of 
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flexible manipulators using LQG controller for tracking of a 
pre-defined trajectory. Lee and Benli [8] designed an optimal 
control law for a flexible robot arm via linearization of 
equations. 

In this paper, finding maximum dynamic load of 
manipulators is considered using closed loop nonlinear 
optimal control approach. For this purpose the nonlinear HJB 
equation, appeared in nonlinear optimal control problem must 
be solved. In general case this equation is a nonlinear partial 
differential equation that several methods are discussed for 
solving it [9]. 

In this paper, two applicable methods are considered for 
finding a solution to nonlinear HJB equation for fully nonlinear 
dynamics of manipulators. In the first method, numerical 
approximation based on Galerkin approach is used for solving 
HJB equation. The explanation and some applications of this 
method for solving optimal control problem are indicated in 
[10, 1 1]. In this method, designing procedure is implemented 
off-line and the execution time and convergence of algorithm 
is dependent on the selection of input parameters. In the 
second method, state-dependent Riccati equation technique 
is employed for finding optimal control law. This method is 
introduced in [12] and then is developed by Wernli and Cook 
[13] and Cloutier [14]. In addition, stability analysis of the 
method is considered in [15] by Hammet and Brett. This 
method is used to design controller for rigid and flexible 
manipulator [16, 17, 18 and 19]. The advantage of this method 
is that computing nonlinear optimal control takes place 
systematically. 

Power series approximation is applied to solve the SDRE 
equation numerically. Finally the DLCC of mobile two-link 
robot and 6R manipulator is determined using these two 
controllers and then results for predefined trajectory are 
demonstrated and simulation results for 6R manipulator 
compared with the experimental test. 

II. Solution of nonlinear optimal control problem 

A. First method: Successive Approximation 

For a system with the following general dynamics equation: 

x=f(x)+g(xju(xj (i) 

With a cost function as: 

J(x)=j* f'tfxj-ufxfRiifxjjdt 

The purpose is finding control law u(x) to minimize the 
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cost function. In (2), l(x) is positive definite that can be 
expressed by x T Qx where Q is a positive semi definite matrix; 
also, R is a positive definite matrix. It can be shown that the 
optimal control law is achieved via solving Hamilton- Jacobi- 
Bellman (HJB) eauation, which has the following form: 

—f($^-—g pc) K i g<d—=0 

ex 4 ex ex 



With finding the solution of (3), the optimal control law is 
achieved as follow: 

2 3x 

Equations (3) and (4) can be rewritten as follows: 



ol la^ 
(4) 



£t 



■[/+Sa <s; ]+/+a <Sjr «a^ = 



2 6 fir 



(5) 
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Equations (5) and (6) are solved by iteration method with 
an initial value u (°> . Equation (5) is known as generalized 
Hamilton- Jacobi-Bellman (GHJB). The analytical solution of 
the GHJB is not possible and it is solved by numerical methods 
such as Galerkin method. According to the Galerkin method, 

the cost function f l) (x) for each step can be expanded as a 
combination of specific basis functions, shown 



as{ (/>j(x)f . These basis functions are selected so that 
the state variables are continuous and bounded in reign q . 
So the function f l) (x)i& rewritten as follow: 

^ = ±C^%( X J (?) 

An approximation of J l) (x) for numerical solution is the finite 
number of basis functions as 



where N is the order of approximation: 



(S) 



Substituting the (8) in (5), the error of approximation can be 
written as: 



f>(X)~ 



#«) 



3x 



(f+&£' y )+i+iI i>T R£'> (9) 



According to the Galrkin method, the inner product of </>j 
and the error function must be zero: 



<eV(x),$/x)> a =0j = l.^N 



(10) 



Where the inner product of two functions f and g is defined 
as follow: 



< f(x),g(x) >e= j s f(x)g(jc)dx. 



(11) 
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Coefficients vector d i} =[d/ ) ,d 2 i) ,...,(% ) fis computed 

through solving (10) and then u <^ is achieved via (6). 

The iteration process is repeated until the value of same 
coefficients become equal in during two steps with 
approximation accuracy. 
It must be noted that the initial control vector u (°) is 

selected so that the system be stable and all vectors u (i) 
computed in the next steps have the same property with 
the difference that control u (i+1) has better performance 
than M ^[ll]. 

B. Second method: State-dependent Riccati equation 

Using State-dependent Riccati equation technique to 
design nonlinear optimal control law, the function f(x) in 
equation (1) must been parameterized as below: 



f(x)=jl(x)jL 



(12) 



That the matrix A (x) is nonlinear function of state variables. 
Then, SDRE equation is formulated as: 

rfx)J(x)+J(xfp(x)-rfxtex)R<g(xfp(x)+Q=Q (B) 

This equation must be solved for a positive definite state 
dependent matrix p(x). The nonlinear optimal feedback law is 
established as: 

The challenge of the SDRE method is finding the solution of 
(13) that usually is difficult to get it analytically, so it can be 
numerically solved. 

For solving the SDRE equation power series approximation 
is employed, for this purpose equations of system are 
rewritten as: 



x = A(xjx+B(xju 
A(x) andp(x) are rewritten as series: 

.-•■ 



(15) 



(17) 



Inserting A(x) and/?(x) in (13), L Q and L\ are computed 
using following equations [9]: 

1^^41,-1,^41,-0=0 (IS) 

Where (18) is the Algebraic Riccati equation and (19) are 
Lyapunov equations. Then the optimal control law is 
calculated via (14). 
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III. Dynamic Modeling of manipulators 

A. Two-link mobile manipulator 

Consider a two-link mobile manipulator with a schematic 
view shown in fig. 1. 



*MvJ*) 




Figure 1. Mobile two-link manipulator. 

Parameters of manipulator are shown in the table I [20]. 

TABLE I.Parameters of mobile manipulator 



Parameters 


YahiE 


Unit 


Leneth of links 


U=U=&5 


tn 


Center of mass 


Le2=Lcl=0.25 


m 


Mass of links 


ml=Xm2=3 


Eg 


Moment o f inertia 


11 =0.41 6 r 
12=0.0625 


Kg.*? 


Mass of wheel 


; 


^ 


Mass of base 


94 


Eg 


Moment of 
-inertia of base 






6.609 




Eg.**' 


Moment of 
inertia of wheels 




~QB1 ' 

fifl 
_ QB1_ 




Kg-rf 


6 


0.171 


m 


r 


0.075 


nt 


L6 


0.4 


m. 



As described in [20], Dynamic equations of motion are 
obtained using Lagrange method: 



F x ~ 




~Ju 


J 12 


J 13 


Ju 


111' 


~x f ~ 




~c; 


F y 




Jn 


J 22 


^23 


^24 


**25 


y f 




c 2 


T 


= 


J,s 


^23 


J 33 


J34 


J33 


% 


+ 


C3 


h 




Ju 


J 24 


J34 


J 44 


J43 


di 




c 4 


T 2_ 




[J 1S 


J 25 


J33 


J43 


J 5S \ 


kJ 




kJ 



(20) 



Parameters in (20) are presented in [20]. The degree of free- 
dom of end effector is two, and the degree of freedom of 
system implied by (20) is five, thus the order of redundancy of 
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the system is three that is the number of required constraints, 
which must be applied for redundancy resolution. The num- 
ber of nonholonomic constraints of the system is one and it 
is the form of [20]: 

* f sm(P )- Jy »<<%)+ Vt =0 £1) 

Two holonomic constraints that apply to system are the 
predefined path for the base coordinates x f ,y fm Then 

x f ,y f ,x f , y f are gained and 6 , 6 , 6 f are achieved from (21). 

With these assumptions, the equations of the system (20) 
can be rewritten as follow: 



r z. 
Where: 



' it 



p" 


- 


r / 


kJ 




kJ 



(22) 



R ; = J,J f + J M y f + jJ + C< 



(23) 



J?j = J,Sf ■+ J J* j'f ■+ J I: .6 + C, 
For finding state space equations, state variables are 
chosen as: 



X = 



PC," 






&- 









(24) 



P5) 



Then state space representation is: 

jc 4 = P(-J 45 (U r R 1 )+ J«(U,-RJj 
Where 

B. 6R FIXED ROBOT 

Consider a six degree of freedom robot that is shown in Fig. 
2 [21]. 




Fig 2. 6R robot 

The dynamic equation of motion of this robot can be 
obtained as: 

T(t) = D(q(tJMt)+C(q(t) ? ^tj)^G(q(t)) QG) 



q is the vector of angular position of joints: 

(27) 
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Angular positions and velocities of links are selected as state 
variables; therefore, the state-space representation is obtained 
as: 

X = [x r ;x M ;x s ;x Jff ;x J£ ;x J2 ;IT s (U-C-G)J p£) 

In (28), D, C and G are the inertial matrix, the vector of Coriolis 
and centrifugal forces and the gravity force vector, 
respectively, t/in this equation is the input control vector. 

IV. Simulation and experimental results 

A. Case study 1 

The function l(x) in (2) is considered as: 

l( x) = *f -x; -x: -x; (29) 

And matrix R is selected I 2x2 for both methods. Basis 
functions required for the Galerkin algorithm are selected as: 

| $. | "* = |xf , x^ ,x* a x^, x^x 3 , x^ , XjX 4 .jyc, ^^ a ij} (30) 
Limits of state variables are: 
- 3 n rad <x } < +3n rad 



-4 rad/ sec <x 2 < +4 rad/ sec 
0. 1 rad <x 3 < 3 rad 
-4 rad/ sec <x 4 < + 4 r ad/ sec 



(31) 



An initial control vector ii 0) (x) required in S A method is design 
using LQ technique: 



(32) 



The upper and lower bounds of torque of motors are as 
follows: 







x 


M 


"-3.16,0,-7.65,-1.09' 


X 


u 2 J 


0,-3.16,-1.09,-3.67 


X 



(33) 



*£ = %-*& 

Where kj —^ s ^ 2 =z s /w d , also Z s is stall torque and 

W nl is no load speed of motor. 

Two controllers based on S A and SDRE methods are designed 
for two- link mobile manipulator to track a square trajectory 
with 2cm allowable tracking error. Trajectories using two 
methods and related variations of tracking error are presented 
in Fig. 3. 



4 
0.2 

-D.2 
-0.4 



-SDRE 
-3A 




f£ 12 



Fig 3. End effector trajectory and Tracking error. 

Dynamic load carrying capacity of manipulator is obtained 
5 kg using controller based on SA method and 5.9 kg for 
SDRE controller. The results demonstrate that the DLCC 
obtained using SDRE controller is higher than using SA 
controller. Necessary torques of joints for trajectory tracking 
is shown in Fig. 4. 




Fig 4. Motor torque of links. 
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(35) 



A. Case study 2 

As second study, a circular trajectory is selected for 
tracking problem of 6R robot. The initial angular positions of 
links are: 

^ +GB -OB Q& Q7 -fi/2 -«-/2f (34) 

Equations of desired the circular trajectory that must be 
happened in 2n seconds are: 

x a ,=0.2cos(t)+0.1 
y a . =0.05 sin(t)- 0.45 
Zj=0.1co5(t)+Q.3 

Because the second method reaches better DLCC and it can 
be implemented systematically, a nonlinear optimal controller 
is designed using SDRE method for simulation study 

The function l(x) in (2) is considered as standard form xQx 

where Q = I ]2xl2 and matrix R is selected to be I 6x6 . 

For an allowable tracking error equal to 2cm, the DLCC of 
robot is computed 0.9 kg. The value of the DLCC is a function 
of matrixes Q and R, the value of admissible error and the 
characteristics of motors as (33). 

Fig. 5 shows three different trajectories for end effector: 
desired trajectory, trajectories obtained in simulation and 
experimental test. 




— -Desired 
— u Exp&imert 



Fig 5. End effector path during tracking. 

Figure (6a) presents simulation results for angular 
positions of joints in full load conditions. This figure indicates 
smooth angular motion for joints during the motion. Figure 
(6b) shows experimental test results of angular position of 
links. 




Fig 6. Angular position of joints: a. simulation, b. experimental 

test. 

Conclusion 

In this paper, two nonlinear methods are applied for 
designing nonlinear optimal controller of both fixed and 
mobile manipulators and determining dynamic load carrying 
capacity of them that is an important characteristic of a 
manipulator. The results indicate that the system response is 
appropriate and acceptable. It should be noted that the 
structure of controller is nonlinear feedback of the state 
variables, in other words it is a closed loop control system. It 
should be noted that in Successive Approximation method, 
various selections of Q and different number and type of 
basis functions will result different controllers with different 
properties that is one of the advantages of this method. 
Determining the basis functions is the main point of method 
because that the convergence of Galerkin algorithm is depend 
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on the type of basis functions and the number of these 
functions determines the required memory and speed of 
numerical calculations. A usual way for selecting basis 
functions is try and error procedure. Another point that 
should be mentioned is that the calculations of S A method 
are done automatically and Off-line by a computer. However 
State Dependent Riccati Equation method provides a 
systematic way to deal with nonlinear control systems. Extra 
design degrees of freedom arising from the various 
parameterizations in the form of (14) also the performance of 
designed controller is related on the selection of R and Q 
matrixes. 
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